package util;

public class Matrix_3X3
{

	private double[][] values;
	public Matrix_3X3(int dimX, int dimY)
	{
		values = new double[dimX][dimY];
	}
	
	public double get(int x, int y) { return values[x][y]; }
	public void set(int x, int y, double v) { values[x][y] = v; }
	
	// algorithm from: http://en.wikipedia.org/wiki/Eigenvalue_algorithm#3.C3.973_matrices
	public static double[] computeEigenvalues(double[][] A)
	{
		double[] eigenvalues = new double[3];
		
		double p = A[0][1]*A[0][1] + A[0][2]*A[0][2] + A[1][2]*A[1][2];
		
		// A is diagonal
		if (p == 0)
		{
			eigenvalues[0] = A[0][0];
			eigenvalues[1] = A[1][1];
			eigenvalues[1] = A[2][2];
		}
		else
		{
			double q = trace(A)/3;
			p = (A[0][0] - q)*(A[0][0] - q) + (A[1][1] - q)*(A[1][1] - q) + (A[2][2] - q)*(A[2][2] - q) + 2*q;
			p = Math.sqrt(p / 6);
			
			// compute B = (A - q * I) / p where I is the identity matrix 
			double[][] B = new double[3][3];
			for (int i = 0; i < 3; i++)
				for (int j = 0; j < 3; j++)
					B[i][j] = A[i][j];
			
			// identity matrix only has 1's in a diagonal so we only need to subtract q from B fromt he diagonal
			B[0][0] -= q;
			B[1][1] -= q;
			B[2][2] -= q;
			
			double r = determinant(B) / 2;


			// In exact arithmetric for a symmetric matrix  -1 <= r <= 1 but computation error can leave it slightly outside this range.
			double phi;
			if (r <= -1) phi = Math.PI / 3;
			else if(r >= 1) phi = 0;
			else phi = Math.acos(r)/3;
			
			eigenvalues[0] = q + 2 * p * Math.cos(phi);
			eigenvalues[2] = q + 2 * p * Math.cos(phi + Math.PI * (2/3));
			eigenvalues[1] = 3 * q - eigenvalues[0] - eigenvalues[2];
		}
		
		return eigenvalues;
	}
	
	public static double trace(double[][]A)
	{
		double Tr = 0.0;
		for (int i = 0; i < A.length; i++) Tr+= A[i][i];
		return Tr;
	}
	
	public static double[][] transpose(double[][] A)
	{
		double[][] T = new double[3][3];
		T[0][0] = A[0][0];
		T[0][1] = A[1][0];
		T[0][2] = A[2][0];
		
		T[1][0] = A[0][1];
		T[1][1] = A[1][1];
		T[1][2] = A[2][1];
		
		T[2][0] = A[0][2];
		T[2][1] = A[1][2];
		T[2][2] = A[2][2];
		return T;
	}
	
	public static double determinant(double[][] A)
	{
		/*    |a b c|
		 * det|d e f| = aei + bfg + cdh - ceg - bdi - afh 
		 *    |g h i|
		 */
		double a = A[0][0];
		double b = A[1][0];
		double c = A[2][0];
		double d = A[0][1];
		double e = A[1][1];
		double f = A[2][1];
		double g = A[0][2];
		double h = A[1][2];
		double i = A[2][2];
		return a*e*i + b*f*g + c*d*h - c*e*g - b*d*i - a*f*h;
	}
	
	
	public static double[][] multiply(double[][]A, double[][]B)
	{
		double[][] ret = new double[3][3];
		
        for(int i = 0; i <3; i++)
        {
			for(int j = 0; j <3; j++)
			{
				for(int k = 0; k <3; k++)
				{
					ret[i][j] +=A[i][k]*B[k][j];
				}
			}
        }
		
		return ret;
	}
}
